#include "../globals/globals.h"
#include "../pid/pid.h"
#include "auto_lib.h"
////////////////////////以下参数根据机器需要调整
// 微点动最小电压
#define max_point_move_pwr 10
// kp缩放系数
// 4马达底盘原速需要再放大,kp_ratio>1，如1.35;
// 6马达底盘、或加速底盘需要适当缩小,kp_ratio<1，如0.65
#define kp_ratio 0.65
#define D_test 0.01

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#define COUT_TURN                                                              \
  if (0 && t_run.time() < timeout)                                             \
  cout << "###" << t_run.time() << "  "
void turn_pid_adjust_agl_absolute_data_cb_static(void *arg);

bool turn_pid_adjust_force_stop = false;
bool turn_pid_adjust_running = false;

static double pwr = 0; // 用于显示的全局变量

struct Arg_turn_pid_adjust_agl_absolute {
  bool run_done; // 状态标志，是否完成
  double aim_agl;
  double max_pwr;
  int timeout;
  bool waitForCompletion;
  double max_wheel_difference_adjust_pwr;
} arg_turn_pid_adjust_agl_absolute;
//////////////////////////////////////////////////////////////////////////////
void turn_pid_adjust_stop() { turn_pid_adjust_force_stop = true; }
bool is_turn_pid_adjust_done() {
  return arg_turn_pid_adjust_agl_absolute.run_done;
}
////////////////////////////////////////////////////////////////////////////
double get_angle_done_pct(double angle_now, double begin_angle,
                          double aim_angle) {
  double agl_all = aim_angle - begin_angle;
  double angle_done = angle_now - begin_angle;
  return angle_done / agl_all * 100; // 返回百分比,超过目标则返回值大于100。
}

///////////////////////////////////////////////////////////////////////////////
/*
微微点动
*/
///////////////////////////////////////////////////////////////////////////////
void turn_point_move_absolute(double agl_relative, int timeout = 50) {
  double agl_aim_abs = get_gyro() + agl_relative;
  timer t;
  while (1) {
    double pmove_pwr =
        sgn(agl_relative) * max_point_move_pwr - get_gyro_rate() * 0.02;
    if (abs(pmove_pwr) > max_point_move_pwr)
      pmove_pwr = sgn(pmove_pwr) * max_point_move_pwr;

    pwr = pmove_pwr; // 打印追踪
    chassis.FT_set_voltage(0, pmove_pwr);

    if (sgn(agl_relative) == 1) // froward
      if (get_gyro() > agl_aim_abs - 0.08) {
        break;
      }
    if (sgn(agl_relative) == -1) // backward
      if (get_gyro() < agl_aim_abs + 0.08) {
        break;
      }

    if (t.time() > timeout) {
      break;
    }
    wait(2);
  }
  pwr = 0;
  chassis.mtr_stop(brake);
  wait(5);
}
///////////////////////////////////////////////////////////////////////////////
static void print() {
  printf("data = [\n");
  while (!is_turn_pid_adjust_done()) {
    printf("[%5.2f,%5.2f,%5.2f],\n", get_gyro(), get_gyro_rate(), pwr);
    wait(10);
  }
  printf("]\n");
}

/*
启动函数
*/
///////////////////////////////////////////////////////////////////////////////
void turn_pid_adjust_agl_absolute(double aim_agl, double max_pwr, int timeout,
                                  Block waitForCompletion,
                                  double max_wheel_difference_adjust_pwr) {
  // thread tp(print);  ///////////////////////////
  if (timeout < 0) {
    cout << __PRETTY_FUNCTION__ << endl;
    cout << "error: timeout<=0\n\n";
    return;
  }
  // 等待前一個线程结束
  while (turn_pid_adjust_running)
    wait(5);

  arg_turn_pid_adjust_agl_absolute = {false,
                                      aim_agl,
                                      max_pwr,
                                      timeout,
                                      (bool)waitForCompletion,
                                      max_wheel_difference_adjust_pwr};

  turn_pid_adjust_force_stop = false;

  // 建立线程
  thread t(turn_pid_adjust_agl_absolute_data_cb_static,
           &arg_turn_pid_adjust_agl_absolute);

  // 等待启动
  while (!turn_pid_adjust_running)
    wait(5);

  // 等待完成
  if ((bool)waitForCompletion) {
    while (turn_pid_adjust_running && !turn_pid_adjust_force_stop)
      wait(20);
  }
}

// 相对旋转
void turn_pid_adjust_agl_relative(double delta_angle, double max_pwr,
                                  int timeout, Block waitForCompletion,
                                  double max_wheel_difference_adjust_pwr) {
  double aim_agl = get_gyro() + delta_angle;
  turn_pid_adjust_agl_absolute(aim_agl, max_pwr, timeout, waitForCompletion,
                               max_wheel_difference_adjust_pwr);
}

///////////////////////////////////////////////////////////////////////////////
/*
线程执行函数
*/
double get_gyro(); // 获取陀螺仪
double get_gyro_rate();
///////////////////////////////////////////////////////////////////////////////
void turn_pid_adjust_agl_absolute_data_cb_static(void *arg) {
  turn_pid_adjust_running = true;

  // 建立局部变量，供本线程使用
  double aim_agl = arg_turn_pid_adjust_agl_absolute.aim_agl;
  double max_pwr = arg_turn_pid_adjust_agl_absolute.max_pwr;
  int timeout = arg_turn_pid_adjust_agl_absolute.timeout;
  if (timeout == 0)
    timeout = 1000000; // long time enough

  double max_wheel_difference_adjust_pwr =
      arg_turn_pid_adjust_agl_absolute.max_wheel_difference_adjust_pwr;
  ////////////////////////////////////////////////////////////////////////////////////

  timer t;
  timer t_run;
  int begin_dir = sgn(aim_agl - get_gyro());

  double begin_angle;
  double aim_angle = aim_agl;

#define agl_abs_left abs(aim_angle - get_gyro())
  if (agl_abs_left == 0)
    return;

  begin_angle = get_gyro();

  double agl_all_abs = abs(aim_angle - begin_angle);

  /*
    kp: 与角度相关
    180->0.8   90->1.2  45->1.5  10->2.5
  */
  double kp;
  double a = 6.658;
  double b = -0.152;
  double c = -2.174;
  kp = a * pow(agl_abs_left, b) + c;

  // 手动微调kp
  kp *= kp_ratio; //// 4马达底盘原速需要再放大6马达底盘、或加速底盘需要适当缩小

  COUT_TURN << "###kp=" << kp << endl;

  // pid_ctrl pid_angle = pid_ctrl::get_instance_old_version(
  //     kp, 0.013, D_test, 20, aim_angle, 0.3, 5, max_pwr);
    pid_ctrl pid_angle = pid_ctrl::get_instance_old_version(
      kp, 0.013, D_test, 20, aim_angle, 0.3, 5, max_pwr);
  pid_ctrl pid_wheels_speed_adjust = pid_ctrl::get_instance_old_version(
      0.05, 0.0024, 0, 10, 0, 2, 5, max_wheel_difference_adjust_pwr);

  double output_wheel_speed_adjust = 0;
  pwr = 0;
  double agl_err;

  /*
  匀速接近阶段的剩余角度,机器速度快，就需要放大，速度慢就需要缩小
  */
  const double pid_get_close_err = 2;

  // todo:可能需要作为参数使用
  // 45-ok:002 开始降速的剩余角度百分比
  const double pid_get_close_err_pct = 0.02;
  // const double pid_get_close_err_pct =
  //     0.05;  // 45-ok:002 开始降速的剩余角度百分比

  double max_rate_abs_in_speeding = 0; // 加速阶段最大速度
  COUT_TURN << "###1--pid get close begin, agl: " << get_gyro() << endl;
  double rate_now;
  while (!turn_pid_adjust_force_stop && t_run.time() < timeout &&
         agl_abs_left > pid_get_close_err &&
         agl_abs_left > agl_all_abs * pid_get_close_err_pct) {
    rate_now = get_gyro_rate();
    // turn
    agl_err = aim_angle - get_gyro();
    // over run
    if (sgn(begin_dir) != sgn(agl_err))
      break;
    if (abs(rate_now) > max_rate_abs_in_speeding)
      max_rate_abs_in_speeding = rate_now;

    pwr = pid_angle.pid_output_need_err(agl_err);

    output_wheel_speed_adjust = pid_wheels_speed_adjust.pid_output_need_err(
        chassis.get_actual_velocity(-1) + chassis.get_actual_velocity(1));

    // output all
    chassis.LR_set_voltage(pwr - output_wheel_speed_adjust,
                           -pwr - output_wheel_speed_adjust);
    wait(2);
  }
  COUT_TURN << "###1--pid get close end, agl: " << get_gyro()
            << "  max_rate_abs_in_speeding:" << max_rate_abs_in_speeding
            << "  supposed end with gyro at:"
            << aim_agl - agl_all_abs * pid_get_close_err_pct << endl;

  COUT_TURN << "###2--slow down begin, agl: " << get_gyro() << endl;

  double kp_rate_slow_down = 0.25;

  double last_rate = rate_now;
  int dir_reverse_count = 0;
  double kp_reduce_rumble = 1; // 手动减小功率防止不断正反抖动
  while (!turn_pid_adjust_force_stop && t_run.time() < timeout) {
    rate_now = get_gyro_rate();
    agl_err = aim_angle - get_gyro();
    if (abs(agl_err) < 0.25 && abs(rate_now) < 50)
      break;

    if (abs(agl_err) < 0.5 && abs(rate_now) < 25) {
      pwr = -rate_now * 0.3 * kp_reduce_rumble;
    } else {
      if (sgn(agl_err) == sgn(rate_now)) { // getting to aim
        pwr = -kp_rate_slow_down * rate_now * kp_reduce_rumble + agl_err * 1.5;
        abs_max_limit(pwr, max_rate_abs_in_speeding * 0.2);
      } else { // drag back to aim
        pwr = -kp_rate_slow_down * 0.5 * rate_now * kp_reduce_rumble +
              agl_err * 0.95;
        abs_max_limit(pwr, max_rate_abs_in_speeding * 0.15);
      }
    }

    output_wheel_speed_adjust = pid_wheels_speed_adjust.pid_output_need_err(
        chassis.get_actual_velocity(-1) + chassis.get_actual_velocity(1));

    pwr *= kp_reduce_rumble; // 再次缩小电压
    abs_min_limit(pwr, max_point_move_pwr);
    chassis.LR_set_voltage(pwr - output_wheel_speed_adjust,
                           -pwr - output_wheel_speed_adjust);

    if (pwr == max_point_move_pwr && abs(rate_now) < 50)
      break; // 不好调整，直接到点到中
    wait(5);
    // 速率不断反向
    if (sgn(rate_now) != sgn(last_rate)) {
      dir_reverse_count++;
      if (dir_reverse_count >= 2)
        kp_reduce_rumble *= 0.8; // 不断缩小
    }
    last_rate = rate_now;
  }
  COUT_TURN << "###2--slow down end, agl: " << get_gyro() << endl;

  // 3: point move to aim
  COUT_TURN << "###3--point move begin, agl: " << get_gyro() << endl;

  while (!turn_pid_adjust_force_stop && t_run.time() < timeout) {
    agl_err = aim_angle - get_gyro();

    if (abs(agl_err) < 0.4 && abs(get_gyro_rate()) < 25)
      break;

    if (abs(agl_err) > 1.5)
      turn_point_move_absolute(0.35 * sgn(agl_err));
    else
      turn_point_move_absolute(0.25 * sgn(agl_err));

    wait(5);
  }
  COUT_TURN << "###3--point move end, agl: " << get_gyro() << endl;
  // 4: chassis stop steady
  COUT_TURN << "###4--steady begin, agl: " << get_gyro()
            << ", rate:" << get_gyro_rate() << endl;
  pwr = 0;
  chassis.mtr_stop(hold);
  while_wait(abs(get_gyro_rate()) > 10 && t_run.time() < timeout);

  COUT_TURN << "###4--steady end, agl: " << get_gyro()
            << ", rate:" << get_gyro_rate() << endl;

  COUT_TURN << "\n\n### final--agl: " << get_gyro()
            << ", rate:" << get_gyro_rate() << endl;
  COUT_TURN << "### final--time: " << t_run.time() << endl;

  wait(5);
  pwr = 0;
  chassis.mtr_stop(brake);
  wait(5);
  // 设置结束状态
  arg_turn_pid_adjust_agl_absolute.run_done = true;
  turn_pid_adjust_running = false;
  if (t_run.time() > timeout) {
    cout << "--- Center turn";
    cout << ":  timeout end;  "
         << " aim_agl=" << aim_agl << " timeout=" << timeout
         << " final angle=" << get_gyro() << endl;
  }
}
